//
// AStar
//

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;

class SearchAStar implements Search
{
	static boolean debug = false;
	
	protected Heuristic heuristic = new RushHourHeuristic1();//hardcoded now, but should be dynamic
	protected StateCostMap openList = new StateCostMap();
	protected HashMap<Long,StateCostWrapper> closedMap = new HashMap<Long,StateCostWrapper>();//map a closed state with its cheapest parent	
    protected long m_nodesExpanded;

    // Get number of nodes expanded.
	public long getNodesExpanded()
    {
        return m_nodesExpanded;
    }

	public int costFunction(int cost, State s) {
		return cost + heuristic.getHeuristic(s);
	}

	
	// Input: start state
	// Output: cost of solution and the solution path.
	// Returns: true if solution found, otherwise false.
    public boolean run( State s, Cost cost, ArrayList<Action> solution )
    {
    	/* Pseudocode found at: http://wiki.gamegardens.com/Path_Finding_Tutorial
	   create the open list of nodes, initially containing only our starting node
	   create the closed list of nodes, initially empty
	   while (we have not reached our goal) {
	       consider the best node in the open list (the node with the lowest f value)
	       if (this node is the goal) {
	           then we're done
	       }
	       else {
	           move the current node to the closed list and consider all of its neighbors
	           for (each neighbor) {
	               if (this neighbor is in the closed list and our current g value is lower) {
	                   update the neighbor with the new, lower, g value 
	                   change the neighbor's parent to our current node
	               }
	               else if (this neighbor is in the open list and our current g value is lower) {
	                   update the neighbor with the new, lower, g value 
	                   change the neighbor's parent to our current node
	               }
	               else this neighbor is not in either the open or closed list {
	                   add the neighbor to the open list and set its g value
	               }
	           }
	       }
	   }
       	 */
    	heuristic.init(s);    	
    	openList.clear();
    	long initialStateId = s.getStateID();
    	int initialHeuristic = costFunction(0, s);
    	addToOpenList(s, initialStateId, initialStateId, 0);
    	//openList.addState(initialStateId, initialStateId, 0, initialHeuristic);
    	closedMap.clear();
        m_nodesExpanded = 0;
        StateCostWrapper solutionState = null;
        boolean solved = false;
        while(!solved) {
    	    //Cost cheapest = new Cost();
    	    StateCostWrapper cheapestOpenState = openList.getCheapestState(true);
    	    s.setState(cheapestOpenState.stateId);
    	    if(s.isGoal()) {
    	    	solutionState = cheapestOpenState;
    	    	solved = true;
    	    	break;
    	    }
    	    closedMap.put(cheapestOpenState.stateId, cheapestOpenState);
    	    m_nodesExpanded++;
    	    ArrayList<Action> actions = new ArrayList<Action>();
    	    s.getActions( actions );
    	    for(Action action: actions) {
    	    	int actionCost = s.getCost(action);    	    	
    	    	int neighborCost = cheapestOpenState.cost+actionCost;
    	    	s.make(action);
    	    	long neighborStateId = s.getStateID();
	    		StateCostWrapper closedNeighbor = closedMap.get(neighborStateId);
    	    	if(closedNeighbor!=null) {
    	    		if(closedNeighbor.cost > neighborCost) {
		    			closedNeighbor.cost = cheapestOpenState.cost+actionCost;
		    			closedNeighbor.parentStateId = cheapestOpenState.stateId;    	    			
    	    		}
    	    	}
	    		else {
	    			StateCostWrapper openNeighbor = openList.findState(neighborStateId); 
	    			if(openNeighbor!=null) {    	 	               
	    				if(openNeighbor.cost>neighborCost) {
		    				openNeighbor.cost = neighborCost;
		    				openNeighbor.parentStateId = cheapestOpenState.stateId;	    					    				
		    				openList.removeState(openNeighbor.stateId);//remove previous hashing
		    				//openList.addState(openNeighbor);//will be added with new cost
		    				addToOpenList(s, neighborStateId, cheapestOpenState.stateId,
		    						neighborCost);
	    				}
	    			}
	    			else {
	    				addToOpenList(s, neighborStateId, cheapestOpenState.stateId, 
	    						neighborCost);
	    			}
	    		}
    	    	s.retract(action);
    	    }        	
        }
        
        //backtrack to find the solution states and the actions that lead to them        
        int finalCost = 0;
        ArrayList<Long> stateSequence = new ArrayList<Long>();
        long parentStateId = solutionState.parentStateId;
        long stateId = solutionState.stateId;
    	ArrayList<Action> childActions = new ArrayList<Action>();        
        while(stateId!=parentStateId) {
        	s.setState(parentStateId);
        	childActions.clear();
        	s.getActions(childActions);
        	for(Action action: childActions) {
        		int actionCost = s.getCost(action);
        		s.make(action);
        		long childStateId = s.getStateID();
        		if(childStateId==stateId) {        			
        			//find the parent in the closed map
        			finalCost += actionCost;
        			StateCostWrapper parentState = closedMap.get(parentStateId);
        			parentStateId = parentState.parentStateId;
        			stateId = parentState.stateId;
        			solution.add(action);
        			stateSequence.add(childStateId);
        			break;//break out of the inner loop and search for this parent's parent
        		}
        		s.retract(action);
        	}
        }        
        if(debug) {
        	Collections.reverse(stateSequence);
        	for(long seqId: stateSequence) {
        		s.setState(seqId);
        		s.draw();
        	}
        }
    	cost.value = finalCost;
        Collections.reverse( solution );
        System.out.println("Heuristic call count: " + heuristic.getCallCount());        
        return solved;
    }
    
    protected void addToOpenList(State s, long stateId, long parentStateId, int cost) {
		openList.addState(stateId, parentStateId, cost, heuristic.getHeuristic(s));    	
    }
}
